#include "myGps.h"
#include "math.h"

MyGPS::MyGPS(Stream *s) : GPS(s)
{
	FastSerial *fs = (FastSerial *)_port;
	my_lat = my_lon = my_alt = my_speed = my_hdop = my_vdop = my_pdop = my_course = my_deviation = 0.0;
}

void MyGPS::init()
{
	BetterStream *bs = (BetterStream *)_port;
	idleTimeout = 1200;
}

bool MyGPS::read()
{
	bool gotit=false;
	int a = _port->available();
	for (int i=0;i<a;i++)
	{if (_decode(_port->read())){gotit=true;}}
	return gotit;
}

bool MyGPS::_decode(char c)
{
	if (c == '\n' || c == '\r')
	{
		if (sz<9){sz=0;return false;}//$GP___*HH\n --> size of 9
		if (buffer[1]!='G' || buffer[2]!='P' || buffer[6]!=',' || !passChecksum())
		{sz = 0;return false;}//bad packet
		bool gotit=true;
		pos = 7;
		if      (isType("gga")){parse_GGA();}
		else if (isType("rmc")){parse_RMC();}
		else if (isType("gsa")){parse_GSA();}
		else if (isType("gsv")){parse_GSV();}
		else if (isType("gll")){parse_GLL();}
		else if (isType("vtg")){parse_VTG();}
		else {gotit=false;}
		sz = 0;
		return gotit;
	}
	else if (sz > 0 || c=='$'){buffer[sz++]=c;}
	//else: ignore bad charecter
	return false;
}

void MyGPS::parse_GGA()
{
	//Global Positioning System Fix Data
	//$GPGGA,023719.600,3258.5481,N,11704.4769,W,1,8,1.25,252.2,M,-35.2,M,,*6D
	time = getTime();
	my_lat = getLatLon();
	latitude = my_lat*10000000;
	my_lon = getLatLon();
	longitude = my_lon*10000000;
	fix = buffer[pos]>'0';
	skip();
	num_sats = getInt();
	my_hdop = getDouble();
	hdop = my_hdop*10;//TODO: ????UNITS???? Is this right?
	my_alt = getDouble();
	if (buffer[pos]=='M'){altitude=my_alt/100;my_alt*=3.2808399;}
	//else? other unit? do not care for now
	//skip();
	//skip();//Height of geoid above WGS84 ellipsoid ???
	//skip();//unit of ^^^
}
void MyGPS::parse_RMC()
{
	//Recommended minimum specific GPS/Transit data
	//GPRMC,023719.600,A,3258.5481,N,11704.4769,W,0.01,236.96,140612,,,A*76	
	time = getTime();
	fix = buffer[pos]=='A';
	skip();
	my_lat = getLatLon();
	latitude = my_lat*10000000;
	my_lon = getLatLon();
	longitude = my_lon*10000000;
	my_speed = getDouble();
	ground_speed = my_speed*51.4444444;//knots to cm/s
	my_course = getDouble();
	ground_course = my_course*100;
	skip();//date = getDate();
	my_deviation = getDouble();
	if ((buffer[pos]|32)=='e'){my_deviation*=-1.0;}
	//skip();
}
void MyGPS::parse_GSA()
{
	//GPS DOP and active satellites
	//$GPGSA,M,3,12,28,02,10,17,04,09,27,,,,,2.38,1.25,2.02*05
	skip();//mode 2d/3d/auto
	fix = buffer[pos]!='1';
	skip();
	my_active_sat.clear();
	int i;
	for (i=0;i<12 && buffer[pos]!=',';i++)
	{my_active_sat.push_back(getInt());}
	for (;i<12;i++){skip();}
	my_pdop = getDouble();
	my_hdop = getDouble();
	hdop = my_hdop*10;//TODO: UNIT???
	my_vdop = getDouble();
}
void MyGPS::parse_GSV()
{
	//GPS Satellites in view
	//$GPGSV,3,1,11,02,66,184,26,04,64,069,40,09,56,253,20,27,54,232,26*72
	//$GPGSV,3,2,11,12,40,320,26,17,23,057,21,10,19,121,25,28,14,114,34*7C
	//$GPGSV,3,3,11,05,03,169,18,25,02,308,,41,,,*73
	int total,at,amm,rd;
	total = getInt();
	at = getInt();
	amm = getInt();
	if (at == 1){my_vsat_temp.clear();}
	rd = (at == total)?amm%4:4;
	for (int i=0;i<rd;i++)
	{
		Sat s;
		s.prn = getInt();
		s.elev = getInt();
		s.azi = getInt();
		s.snr = getInt();
		my_vsat_temp.push_back(s);
	}
	if (at == total)
	{
		my_vsat = my_vsat_temp;
		my_vsat_temp.clear();
	}
}

void MyGPS::parse_GLL()
{
	//Geographic Position, Latitude / Longitude and time.
	//$GPGLL,3258.5481,N,11704.4769,W,023719.600,A,A*48
	my_lat = getLatLon();
	my_lon = getLatLon();
	latitude = my_lat*10000000;
	longitude = my_lon*10000000;
	time = getTime();
	fix = buffer[pos]=='A';
}

void MyGPS::parse_VTG()
{
	//Track Made Good and Ground Speed.
	//$GPVTG,236.96,T,,M,0.01,N,0.02,K,A*36
	my_course = getDouble();
	ground_course = my_course*100;
	skip();//True
	skip();skip();//Non-existant magnetic compass
	my_speed = getDouble();
	ground_speed = my_speed*51.4444444;//knots to cm/s
}

void MyGPS::skip(){while (buffer[pos++]!=',');}

double MyGPS::getLatLon()
{
	double num,num2;
	if (buffer[pos+4]=='.'){num = (getN()*10)+getN();}
	else {num = (getN()*100)+(getN()*10)+getN();}
	num2 =(getN()*10)+getN();
	pos++;
	num2+=getN()/10.0;
	num2+=getN()/100.0;
	num2+=getN()/1000.0;
	num2+=getN()/10000.0;
	num2*=1.0/60.0;
	num += num2;
	pos++;
	if (buffer[pos]=='S' || buffer[pos]=='W'){num*=-1.0;}
	pos+=2;
	return num;
}

long MyGPS::getTime()
{
	long hr,min,sec;
	hr = (getN()*10)+getN();
	min = (getN()*10)+getN();
	sec = (getN()*10)+getN();
	skip();
	return (hr*3600)+(min*60)+sec;
}

double MyGPS::getDouble()
{
	double num=0.0;
	bool neg = false;
	int p=0;
	bool inDec=false;
	if (buffer[pos]=='-'){neg=true;pos++;}
	while (buffer[pos]!=','&&buffer[pos]!='*')
	{
		if (inDec){num+=((double)getN())/pow(10,++p);}
		else if (buffer[pos]=='.'){inDec=true;pos++;}
		else {num = (num*10.0) + getN();}
	}
	pos++;
	if (neg){num*=-1.0;}
	return num;
}

int MyGPS::getInt()
{
	bool inDec = false;
	bool neg = false;
	if (buffer[pos]=='-'){neg=true;pos++;}
	int num = 0;
	while (buffer[pos]!=','&&buffer[pos]!='*')
	{
		if (!inDec && buffer[pos]!='.')
		{
			num*=10;
			num+=getN();
		}
		else{inDec=true;pos++;}
	}
	pos++;//skip the comma
	if (neg){num*=-1;}
	return num;
}

bool MyGPS::isType(const char *type)
{
	return ((buffer[3]|32) == type[0] && (buffer[4]|32) == type[1] && (buffer[5]|32) == type[2]);
}

bool MyGPS::passChecksum()
{
	if (buffer[sz-3]!='*'){return false;}
	int chka,chkb;
	chkb = (hexToInt(buffer[sz-2])<<4)|hexToInt(buffer[sz-1]);
	chka = 0;
	for (int i=1;i<sz-3;i++){chka ^= buffer[i];}//from $ to *
	return chka==chkb;
}

int MyGPS::hexToInt(char c)
{
	if (c<='9'){return c-'0';}
	if (c<'a'){return c-'A'+10;}
	return c-'a'+10;
}

int MyGPS::getN()
{
	return buffer[pos++]-'0';
}